#include "gimbal.h"
#include "robot_def.h"
#include "dji_motor.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "bmi088.h"
#include "laser.h"

static attitude_t *gimba_IMU_data;      // 云台IMU数据
static DJIMotorInstance *yaw_motor;     // yaw电机
static LaserInstance *laser;            // 激光测距仪

static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

BMI088Instance* imu;

void GimbalInit()
{
    Laser_Init_Config_s laser_config = {
        .usart_config={
            .recv_buff_size = LASER_RECV_SIZE,
            .usart_handle = &huart6,
        }
    };
    laser = LaserInit(&laser_config); // laser初始化用于配合yaw电机寻找目标

    BMI088_Init_Config_s imu_config = {
        .spi_acc_config={
            .GPIOx=CS1_ACCEL_GPIO_Port,
            .cs_pin=CS1_ACCEL_Pin,
            .spi_handle=&hspi1,
        },
        .spi_gyro_config={
            .GPIOx=CS1_GYRO_GPIO_Port,
            .cs_pin=CS1_GYRO_Pin,
            .spi_handle=&hspi1,
        },
        .acc_int_config={
            .exti_mode=EXTI_TRIGGER_FALLING,
            .GPIO_Pin=INT_ACC_Pin,
            .GPIOx=INT_ACC_GPIO_Port,
        },
        .gyro_int_config={
            .exti_mode=EXTI_TRIGGER_FALLING,
            .GPIO_Pin=INT_GYRO_Pin,
            .GPIOx=INT_GYRO_GPIO_Port,
        },
        .heat_pid_config={
            .Kp=0.0f,
            .Kd=0.0f,
            .Ki=0.0f,
            .MaxOut=0.0f,
            .DeadBand=0.0f,
        },
        .heat_pwm_config={
            .channel=TIM_CHANNEL_1,
            .htim=&htim1,
        },
        .cali_mode=BMI088_CALIBRATE_ONLINE_MODE,
        .work_mode=BMI088_BLOCK_PERIODIC_MODE,
    };
    // imu=BMI088Register(&imu_config);
    // gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
    // YAW
    Motor_Init_Config_s yaw_config = {
        .can_init_config = {
            .can_handle = &hcan1,
            .tx_id = 1,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 20,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 2000,
                .DeadBand = 0.3,
            },
            .speed_PID = {
                .Kp = 10,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 4000,
            },
            .current_PID = {
                .Kp = 1,
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 3000,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
            // .other_speed_feedback_ptr=&gimba_IMU_data.wz;
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP | CURRENT_LOOP,
            .reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = GM6020};
    
    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
    yaw_motor = DJIMotorInit(&yaw_config);

    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

    // 设置反馈数据,主要是imu和yaw的ecd
    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round;
    gimbal_feedback_data.laser_distance = laser->laser_measure.laser_distance;

    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}

/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
    // 获取云台控制数据
    // 后续增加未收到数据的处理
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

    // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
    // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
    switch (gimbal_cmd_recv.gimbal_mode)
    {
    // 停止
    case GIMBAL_ZERO_FORCE:
        DJIMotorStop(yaw_motor);
        break;
    // 云台自瞄模式
     // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
    case GIMBAL_GYRO_MODE: // 后续只保留此模式
        DJIMotorEnable(yaw_motor);
        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
    // 云台自由模式,使用编码器反馈
    case GIMBAL_FREE_MODE:
        DJIMotorEnable(yaw_motor);
        DJIMotorOuterLoop(yaw_motor, ANGLE_LOOP); // 切换到角度环
        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED);
        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, MOTOR_FEED);
        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
        break;
    default:
        while (1)
            ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
    }

    // 设置反馈数据,主要是imu和yaw的ecd
    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round;
    gimbal_feedback_data.laser_distance = laser->laser_measure.laser_distance;

    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
}